/*
* @File Name route.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\route\route.c
* @Author: Ruige_Lee
* @Date:   2019-09-09 18:35:00
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-09-26 20:00:48
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/


#include <stdint.h>


#include "realTimePrm/prm.h"
#include "trajectory/trajectory.h"


#include "task/include.h"

extern float poseCheckArray[STEPPER_FIFO_DP][6];
extern uint32_t checkArrayPoint;
extern uint32_t checkArrayMax;

//把下100ms数据压入fifo
static int32_t RP_checkin()
{
	// double PVA[18] = {0};

	static float radAngle[6] = {0.,0.,0.,0.,0.,0.};
	int16_t robotConfig[6];
	
	// for (uint8_t axis = 0; axis < 6; axis ++)
	// {
	// 	radAngle[axis] = stepper_getRad( 0, axis );
	// }

	if ( checkArrayPoint  < checkArrayMax )
	{
		//一次检入10个即100ms
		for ( uint8_t i = 0; i < 10; i++ )
		{
			if ( checkArrayPoint  >= checkArrayMax )
			{
				break;
			}
			for ( uint8_t axis = 0; axis < 6; axis++ )
			{
				robotConfig[axis] = stepper_rad2Pluse( axis, poseCheckArray[checkArrayPoint][axis] - radAngle[axis] );
				radAngle[axis] =  poseCheckArray[checkArrayPoint][axis];
			}
			checkArrayPoint ++;
			while( 0 != stepper_fifoPush( 0, robotConfig ) )	{;}
		}
		
	}




	return 0;
}



//关键常驻耗时函数，检测即将检入的100ms路径是否安全
static int32_t RP_check_step()
{
	uint32_t cAPoint = checkArrayPoint;

	//一次检查10个即100ms
	for ( ; (cAPoint < checkArrayPoint + 10) || ( cAPoint < checkArrayMax ); cAPoint ++ )
	{
		if( 0 == collide_check_top( &poseCheckArray[cAPoint][6] ) )		{;}
		else{return -1;}
	}

	return 0;

}

//关键耗时函数，当目标路径点改变或者常驻检查不通过时会调用，需要留足时间。
//调用时会清空并重刷routePoint
static int32_t RP_route_rePlanning(float targetPose[6])
{
	float staPose[6];
	
	for (uint8_t axis = 0; axis < 6; axis ++)
	{
		staPose[axis] = stepper_getRad( 0, axis );
	}

	// RP_route_init();
	profileCnt = 0;
	if ( 0 == prm_execuse( &staPose[0], &targetPose[0] ))
	{

		uint16_t routerCnt = 0;
		//front point
		for ( uint16_t i = 0; i < MAX_CHECKPOINT; i ++ )
		{
			if ( routeIndex[i] == NULL )
			{
				continue;
			}

			for ( uint8_t axis = 0; axis < 6; axis ++ )
			{
				Cspline[0].points[routerCnt].position[axis] = (routeIndex[i])[axis];
			}
			routerCnt ++;
		}
		//back point
		for ( uint16_t i = MAX_CHECKPOINT; i < 2 * MAX_CHECKPOINT; i ++ )
		{
			if ( routeIndex[i] == NULL )
			{
				break;
			}
			for ( uint8_t axis = 0; axis < 6; axis ++ )
			{
				Cspline[0].points[routerCnt].position[axis] = (routeIndex[i])[axis];
			}
			routerCnt ++;
		}

		//check in point cnt
		if ( routerCnt == 2 )
		{
			for ( uint8_t axis = 0; axis < 6; axis ++ )
			{
				Cspline[0].points[2].position[axis] = Cspline[0].points[1].position[axis];
				Cspline[0].points[1].position[axis] 
					= (Cspline[0].points[1].position[axis] + Cspline[0].points[0].position[axis]) / 2;
			}
			routerCnt ++;
		} 
		Cspline[0].count = routerCnt;

		//load time
		tra_autoSet_timePoint( 0 );
		tra_checkIn(0);

		return 0;
	}
	else
	{
		return -1;
	}
}



extern uint8_t rePlaning;
//100ms 调用一次
int32_t RP_execute( float targetPose[6] )
{
	int32_t status = 0;

	//如果目标点发生改变，则重新规划
	if ( 0 == rePlaning )
	{
		//周期性检查
		status = RP_check_step();

		if ( 0 == status )
		{
			RP_checkin();
		}
		else
		{
			if ( 0 == RP_route_rePlanning( &targetPose[0]) )
			{
				;
			}
			else
			{
				targetPose[0] = 0.;
				targetPose[1] = 0.;
				targetPose[2] = 0.;
				targetPose[3] = 0.;
				targetPose[4] = 0.;
				targetPose[5] = 0.;
				RP_route_rePlanning( &targetPose[0]);
			}
			RP_checkin();//直接检入，不再检查
		}
	}
	else
	{
		if ( 0 == RP_route_rePlanning( &targetPose[0]) )
		{
			;
		}
		else
		{
			targetPose[0] = 0.;
			targetPose[1] = 0.;
			targetPose[2] = 0.;
			targetPose[3] = 0.;
			targetPose[4] = 0.;
			targetPose[5] = 0.;
			RP_route_rePlanning( &targetPose[0] );
		}
		RP_checkin();
		rePlaning = 0;
	}

	
	return 0;

}






